#include "engineering_detect_node.hpp"
#include "detector.hpp"

EngineeringDetectNode::EngineeringDetectNode() : Node("engineering_detector")
{
  RCLCPP_INFO(get_logger(),"Starting EngineeringDetectNode");
  detector_ = initDetector();
  is_debug = declare_parameter("engineering.is_debug", true);
  if(is_debug == true){
    //createDebugPublishers();
  }

  target_pub_  = this->create_publisher<engineering_interfaces::msg::Target>("engineer_detector/target", 
                                                                              rclcpp::SensorDataQoS());
  cam_info_sub_ = this->create_subscription<sensor_msgs::msg::CameraInfo>(
    "camera_info",
    rclcpp::SensorDataQoS(),
    [this](sensor_msgs::msg::CameraInfo::SharedPtr camera_info) {
      std::vector<cv::Point3f> object_points;
      object_points.push_back(cv::Point3f(144.0/1000.0,-144.0/1000.0,0.0));//右上角
      object_points.push_back(cv::Point3f(144.0 / 1000.0, 144.0 / 1000.0, 0.0));//右下角
      object_points.push_back(cv::Point3f(-144.0 / 1000.0, 144.0 / 1000.0, 0.0));//左下角
      object_points.push_back(cv::Point3f(-144.0 / 1000.0,-144.0 / 1000.0, 0.0));//左上角
      cam_center_ = cv::Point2f(camera_info->k[2], camera_info->k[5]);
      // Setup pnp solver
      pnp_solver_ = std::make_unique<PNP_Slove>(camera_info->k, camera_info->d,object_points);
      cam_info_sub_.reset();
    });

    buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
    listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_,this);

    img_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
      "image_raw",
      rclcpp::SensorDataQoS(),
      std::bind(&EngineeringDetectNode::imageCallback, this, std::placeholders::_1));

}

std::unique_ptr<Detector> EngineeringDetectNode::initDetector(){

  auto detector = std::make_unique<Detector>();
  detector->max_angle = declare_parameter("engineering.max_angle", 130);
  detector->min_angle = declare_parameter("engineering.min_angle", 65);
  detector->min_side_ratio = declare_parameter("engineering.min_side_ratio", 0.6);
  detector->max_Pythagorean_diff = declare_parameter("engineering.max_Pythagorean_diff", 2000);
  detector->min_Area_hull_traingle_ratio = declare_parameter("engineering.min_Area_hull_traingle_ratio", 0.7);
  detector->max_AreaHull = declare_parameter("engineering.max_AreaHull", 8000);
  detector->min_AreaHull = declare_parameter("engineering.min_AreaHull", 300);
  detector->min_point_hull_ratio = declare_parameter("engineering.min_point_hull_ratio", 0.5);
  detector->min_hull_size = declare_parameter("engineering.min_hull_size", 5);
  detector->min_Area_rect_ratio = declare_parameter("engineering.min_Area_rect_ratio", 0.1);
  
  return detector;
}


void EngineeringDetectNode::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr img_msg){
  try{
    rclcpp::Time target_time = img_msg->header.stamp;
    const auto transform = buffer_->lookupTransform(
      "baselink","camer",target_time,rclcpp::Duration::from_seconds(0.01));
    const auto& translation = transform.transform.translation;
   baselink_to_camera_.translation = Eigen::Vector3d(translation.x,translation.y,translation.z);
    tf2::Quaternion tf_q;
    tf2::fromMsg(transform.transform.rotation,tf_q);
    tf2::Matrix3x3 tf2_matrix = tf2::Matrix3x3(tf_q);
    for (int i = 0; i < 3; ++i) {
      for (int j = 0; j < 3; ++j) {
        baselink_to_camera_.rotation(i, j) = tf2_matrix[i][j]; // 注意 tf2 的行列索引方式
      }
    }
} catch (tf2::TransformException &ex) {
    RCLCPP_ERROR(get_logger(),"错误:%s",ex.what());

  }


  cv::Mat frame = cv_bridge::toCvShare(img_msg, "rgb8")->image;
  cv::Mat binary_img = detector_->PorcessImage(frame);
  triangles_ = detector_->GetTriangles(binary_img);
  if(detector_->select_final_targets(final_targets_)){
    cv::Mat rvec(3, 1, CV_64F), tvec(3, 1, CV_64F);
			cv::Mat Rvec;
			cv::Mat_<float> Tvec;
			if (pnp_solver_->solvePnp(detector_->targets_, rvec, tvec)) {

				rvec.convertTo(Rvec, CV_32F);  // 旋转向量转换格式
				tvec.convertTo(Tvec, CV_32F); // 平移向量转换格式 
				cv::Mat_<float> rotMat(3, 3);
				cv::Rodrigues(Rvec, rotMat);
				cv::Mat P_oc;
				P_oc = -rotMat.inv() * Tvec;
				std::cout << "P_oc" << std::endl << P_oc << std::endl;

			}
  }



}